#include <math.h>
#include <stdio.h>
#include "Quaternion.h"
#define PI 3.14159265358979323846

void euler_angle_matrix_update(Quaternion_TypeDef* q,double rx,double ry,double rz)
{
    q->rx=rx;
    q->ry=ry;
    q->rz=rz;
}

void euler_to_quaternion(Quaternion_TypeDef* q) 
{
    // 将欧拉角从度转换为弧度
    float rx = q->rx * PI / 180.0;
    float ry = q->ry * PI / 180.0;
    float rz = q->rz * PI / 180.0;

    // 计算各个半角的正弦和余弦值
    float c1 = cosf(rx / 2.0);
    float s1 = sinf(rx / 2.0);
    float c2 = cosf(ry / 2.0);
    float s2 = sinf(ry / 2.0);
    float c3 = cosf(rz / 2.0);
    float s3 = sinf(rz / 2.0);

    q->w = c1 * c2 * c3 - s1 * s2 * s3;
    q->x = s1 * c2 * c3 + c1 * s2 * s3;
    q->y = c1 * s2 * c3 - s1 * c2 * s3;
    q->z = c1 * c2 * s3 + s1 * s2 * c3;
}
